#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav_msg_pkg/action/progress.hpp"

using nav_msg_pkg::action::Progress;
class ProgressActionServer : public rclcpp::Node
{
public:
    ProgressActionServer() : Node("progress_action_server")
    {
        RCLCPP_INFO(this->get_logger(), "Progress action server started");
        progress_action_server_ = rclcpp_action::create_server<Progress>(
            this,
            "get_sum",
            std::bind(&ProgressActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&ProgressActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&ProgressActionServer::handle_accepted, this, std::placeholders::_1));
    }

private:
    /**
     * std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
     * 目标处理
     */
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const Progress::Goal> goal)
    {
        // 不使用的参数
        (void)uuid;
        RCLCPP_INFO(this->get_logger(), "Received goal request with num= %d", goal->num);
        // 校验是否接收
        if (goal->num <= 1)
        {
            RCLCPP_INFO(this->get_logger(), "提交参数被拒绝,请输入大于1的数字");
            return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    /**
     * std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
     * 取消处理
     */
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    /**
     * std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
     * 主逻辑
     */
    void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
        // 子线程处理耗时操作，避免阻塞主线程
        // rclcpp::executors::SingleThreadedExecutor executor;
        // executor.add_node(this->get_node_base_interface());
        std::thread(std::bind(&ProgressActionServer::execute, this, goal_handle)).detach();
    }

    void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
        // 生成连续反馈，返回给客户端
        // publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
        // goal_handle->publish_feedback(feedback_msg);
        int num = goal_handle->get_goal()->num;
        int sum = 0;
        auto feedback_msg = std::make_shared<Progress::Feedback>();
        auto result = std::make_shared<Progress::Result>();

        // 设置休眠时间
        rclcpp::Rate rate(1);
        for (int i = 0; i <= num; i++)
        {
            sum += i;
            double progress = (double)i / num; // 计算进度
            feedback_msg->progress = progress;
            goal_handle->publish_feedback(feedback_msg);
            RCLCPP_INFO(this->get_logger(), "Publish Feedback 进度=%.2f,sum=%d", progress, sum);
            // 检查客户端状态
            if(!goal_handle->is_active()){
                result->sum = sum;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal Canceled, sum= %d", sum);
                return;
            }
            // 处理取消请求
            if (goal_handle->is_canceling())
            {
                result->sum = sum;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal Canceled, sum= %d", sum);
                return;
            }
            rate.sleep();
        }

        // 生成最终响应结果，返回给客户端
        if (rclcpp::ok())
        {
            result->sum = sum;
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal Succeeded, sum= %d", sum);
        }
    }

    rclcpp_action::Server<Progress>::SharedPtr progress_action_server_;
};
// ros2 action send_goal /get_sum nav_msg_pkg/action/Progress -f "{'num':5}"
int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ProgressActionServer>());
    rclcpp::shutdown();
    return 0;
}